package oceanlife.pathfinding;

/*
 * Copyright (c) 2007 by L. Paul Chew.
 *
 * Permission is hereby granted, without written agreement and without
 * license or royalty fees, to use, copy, modify, and distribute this
 * software and its documentation for any purpose, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 */

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;
import java.util.PriorityQueue;
import java.util.Queue;
import java.util.Set;

/**
 * Straightforward undirected graph implementation.
 * Nodes are generic type N.
 *
 * @author Paul Chew
 *
 * Created November, December 2007.  For use in Delaunay/Voronoi code.
 *
 */
public class PointGraph {
	
    private Map<Pnt, Map<Pnt, Edge>> theNeighbors =    // Node -> adjacent nodes
        new HashMap<Pnt, Map<Pnt, Edge>>();
    public Map<Pnt, Map<Pnt, Edge>> getTheNeighbors() {
		return theNeighbors;
	}
    
    private Map<Pnt, Edge> dijkstra = new HashMap<Pnt,Edge>();

	private Set<Pnt> theNodeSet =              // Set view of all nodes
        Collections.unmodifiableSet(theNeighbors.keySet());

	public Edge getShortestPath(Pnt start,Pnt goal, double radius){
		dijkstra.clear();
		dijkstra.put(goal,null);
		dijkstra.put(start, new Edge(new Pnt[]{start}));
		PriorityQueue<Edge> fifo = new PriorityQueue<Edge>();
		fifo.add(dijkstra.get(start));
		double maxDist = Double.MAX_VALUE;
		while(fifo.size() > 0){
			Edge currentEdge = fifo.poll();
			Pnt currentPnt = currentEdge.getLastPoint(); 
			if(!currentPnt.equals(goal)){
				if(currentEdge.getWeight() <= maxDist){
					if(theNeighbors.get(currentPnt) == null){
						System.out.println("NULLLLLLLLLL");
					}
					if(!theNodeSet.contains(currentPnt)){
						System.out.println("theNodeSet null");
					}
					Pnt[] neighs = theNeighbors.get(currentPnt).keySet().toArray(new Pnt[0]);
					for(Pnt adjacentNode : neighs){
						//Edge currentEdge = dijkstra.get(currentPnt);
						if(currentEdge.getPointsOnEdge().length == 1 || !(currentEdge.getSecondLastPoint().equals(adjacentNode))){
							Edge edge2 = theNeighbors.get(currentPnt).get(adjacentNode);
							Edge combi = currentEdge.combine(edge2);
							//System.out.println("   current "+currentEdge.getLength()+" "+currentEdge.getEfficiency()+" "+currentEdge.getWeight());
							//System.out.println("   edge "+edge2.getLength()+" "+edge2.getEfficiency()+" "+edge2.getWeight());
							//System.out.println("   combi "+combi.getLength()+" "+combi.getEfficiency()+" "+combi.getWeight());
							//System.out.println();
							if(!combi.getLastPoint().equals(adjacentNode)){
								combi = combi.reverse();
							}
							Edge best = dijkstra.get(adjacentNode);
							if((best == null || combi.getWeight() < best.getWeight()) && combi.getMinimumWidth() >= 2*radius+4.0){
								dijkstra.put(adjacentNode, combi);
								fifo.add(combi);
								//System.out.println();
							}
						}
					}
				}
			} else {
				maxDist = currentEdge.getWeight();
			}
		}
		return dijkstra.get(goal);
	}

	public PointGraph(PointGraph pG){
	    Map<Pnt, Map<Pnt, Edge>> NeighboursCopy = new HashMap<Pnt, Map<Pnt, Edge>>();
		for(Pnt key : pG.theNeighbors.keySet().toArray(new Pnt[0])){
			NeighboursCopy.put(key, new HashMap<Pnt, Edge>());
			for(Pnt innerKey : pG.theNeighbors.get(key).keySet().toArray(new Pnt[0])){
				Edge ed = pG.theNeighbors.get(key).get(innerKey);
				NeighboursCopy.get(key).put(innerKey,new Edge(ed));
			}
		}
		this.theNeighbors = NeighboursCopy;
	}
    public PointGraph() {
		// TODO Auto-generated constructor stub
	}
	/**
     * Add a node.  If node is already in graph then no change.
     * @param node the node to add
     */
    public void add (Pnt node) {
        if (theNeighbors.containsKey(node)) return;
        theNeighbors.put(node, new HashMap<Pnt, Edge>());
     }

    public int size (){
    	return theNodeSet.size();
    }
    /**
     * Add a link. If the link is already in graph then no change.
     * @param nodeA one end of the link
     * @param nodeB the other end of the link
     * @throws NullPointerException if either endpoint is not in graph
     */
    public void addEdge (Pnt nodeA, Pnt nodeB, Edge edge) throws NullPointerException {
    	;
    	//Map<N, Set<S>> test = theNeighbors.get(nodeA);
    	//Set<S> s = new HashSet<S>();
    	if(edge.getFirstPoint().equals(nodeA)){
	    	theNeighbors.get(nodeA).put(nodeB, edge.reverse());
	        theNeighbors.get(nodeB).put(nodeA, edge);
    	} else {
	    	theNeighbors.get(nodeA).put(nodeB, edge);
	        theNeighbors.get(nodeB).put(nodeA, edge.reverse());
    	}
    }

    /**
     * Remove node and any links that use node. If node not in graph, nothing
     * happens.
     * @param node the node to remove.
     */
    public void remove (Pnt node) {
        if (!theNeighbors.containsKey(node)) return;
        for ( Pnt neighbor : theNeighbors.get(node).keySet() ){
        	theNeighbors.get(neighbor).remove(node);
           // theNeighbors.get(neighbor).remove(node);    // Remove "to" links
        }
        theNeighbors.get(node).clear();                 // Remove "from" links
        theNeighbors.remove(node);                      // Remove the node
    }

    /**
     * Remove the specified link. If link not in graph, nothing happens.
     * @param nodeA one end of the link
     * @param nodeB the other end of the link
     * @throws NullPointerException if either endpoint is not in graph    
     */
    public void remove (Pnt nodeA, Pnt nodeB) throws NullPointerException {
        theNeighbors.get(nodeA).remove(nodeB);
        theNeighbors.get(nodeB).remove(nodeA);
    }

    /**
     * Report all the neighbors of node.
     * @param node the node
     * @return the neighbors of node
     * @throws NullPointerException if node does not appear in graph
     */
    public Set<Pnt> neighbors (Pnt node) throws NullPointerException {
        return Collections.unmodifiableSet(theNeighbors.get(node).keySet());
    }
    
    /**
     * Returns an unmodifiable Set view of the nodes contained in this graph.
     * The set is backed by the graph, so changes to the graph are reflected in
     * the set.
     * @return a Set view of the graph's node set
     */
    public Set<Pnt> nodeSet () {
        return theNodeSet;
    }

	public boolean nodeWith2Branches() {
		for(Pnt key : this.theNeighbors.keySet()){
			Map<Pnt,Edge> node = this.getTheNeighbors().get(key);
			if(node.size() == 2){
				return true;
			}
		}
		return false;
	}

	public static void main(String[] args){
		ArrayList<Line2> lines = new ArrayList<Line2>();
		lines.add(new Line2(new Pnt(0.0,0.0),new Pnt(1.0,0.0)));
		lines.add(new Line2(new Pnt(1.0,0.0),new Pnt(2.0,0.0)));
		lines.add(new Line2(new Pnt(2.0,0.0),new Pnt(3.0,0.0)));
		lines.add(new Line2(new Pnt(0.0,1.0),new Pnt(1.0,1.0)));
		lines.add(new Line2(new Pnt(1.0,1.0),new Pnt(2.0,1.0)));
		lines.add(new Line2(new Pnt(2.0,1.0),new Pnt(3.0,1.0)));
		lines.add(new Line2(new Pnt(1.0,0.0),new Pnt(1.0,1.0)));
		lines.add(new Line2(new Pnt(2.0,0.0),new Pnt(2.0,1.0)));
		
	    PointGraph graph = new PointGraph();
		for(Line2 ent : lines){
			graph.add(ent.getStart());
			graph.add(ent.getEnd());
			graph.addEdge(ent.getStart(), ent.getEnd(), new Edge(new Pnt[]{ent.getStart(),ent.getEnd()}));
		}
		
		Edge path = graph.getShortestPath(new Pnt(0.0,0.0), new Pnt(3.0,1.0), 0.1);
		for(int i = 0 ; i < path.getPointsAmount()-1; i++){
			System.out.println(path.getPointsOnEdge()[i]+" "+path.getPointsOnEdge()[i+1]);
		}
		System.out.println(path.getWeight()+" "+path.getLength()+" "+path.getEfficiency());

	}
	
}
